#!/usr/bin/python3
# -*- coding:utf-8 -*-
from datetime import datetime
from multiprocessing import Process, Value, Pipe
import multiprocessing as mp
import os
import serial
import cv2
import configparser
import pickle
import time
import binascii
import numpy
import socket

import sys
sys.path.append("./")
sys.path.append("../")

from vision.Pretreat import Pretreat
from vision.ClassifierVersion import Img2Status
import vision.Camera as Camera

SOLVE_SERVER_HOST = "127.0.0.1"
SOLVE_SERVER_PORT = 2077

def is_start():
    ser = serial.Serial('/dev/ttyS0', 115200)
    while True:
        size = ser.inWaiting()               # 获得缓冲区字符
        if size != 0:
            response = ser.read(size)# 读取内容并显示
            response = binascii.b2a_hex(response).decode("utf-8")
            #print(response)
            if response == '4f':
                print("r")
                break

def rotate_finish():
    ser = serial.Serial('/dev/ttyS0', 115200)
    while True:
        size = ser.inWaiting()               # 获得缓冲区字符
        if size != 0:
            response = ser.read(size)# 读取内容并显示
            response = binascii.b2a_hex(response).decode("utf-8")
            if response == '4f':
                break
            # elif response == "50":
               # break

def getSolution(facelet_str, solve_type=0, time_limit=5):
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
        sock.connect((SOLVE_SERVER_HOST, SOLVE_SERVER_PORT))
        sock.sendall(bytes( ("%d %d %s\n" % (solve_type, time_limit, facelet_str) ).encode('utf-8') ))
        solution = sock.recv(1024)
    solution = str(solution, encoding='utf-8')
    return solution

def getFinalResult(str_result):
    java_res = os.popen(
        "echo \"{0}\" | ../transform/build/transform"
        .format(str_result))
    lis = java_res.read()
    index_ff = lis.index('ff')
    finalRes = lis[index_ff:-7]+'0'
    return finalRes

def trans(s):
    lis = []
    tran = ''
    for word in s.split():
        if len(word) == 1:
            word = '0' + word
            lis.append(word)
        else:
            lis.append(word)
    for element in lis:
        tran = tran + element + ' '
    tran = tran[:-1]
    return tran

def nothing():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 01 56 56 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame2():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 03 59 6d ae 74 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame1():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 05 d2 e5 19 6d ae eb 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame3():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 03 d2 e5 16 cd 00')
    ser.write(hex_str)
    ser.close()

# we may set the default values
# sig_cap = false, sig_ok = false, sig_term = false
# we use pipe to transfer images
# captured image size must be consistent with that used in calibration
def camera_proc(conn, sig_cap, sig_ok, sig_term):
    #
    # Read Camera specific params
    #
    cam_idx_list = Camera.load_camera_params()
    #
    # Init camera
    #
    caps = [cv2.VideoCapture(i) for i in cam_idx_list]

    while True:
        stat = [cap.isOpened() for cap in caps]
        if False not in stat:
            break

    for cap in caps:
        cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M','J','P','G'))
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, Camera.WIDTH)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Camera.HEIGHT)

    sig_ok.value = True
    print("camera ok")
    _itr = 0
    while True:
        frames = [cap.read()[1] for cap in caps]
        # print("cycle", _itr)
        _itr += 1
        if sig_cap.value == True:
            print("capture!")
            images = [frame.copy() for frame in frames]
            conn.send(images)
            sig_cap.value = False
        if sig_term.value == True:
            break
    print("release captures...")
    for cap in caps:
        cap.release()

def main():
    #
    # Start Image Capture Process
    #
    start_time = datetime.now()
    pics = []
    sig_cap = Value('b', False)
    sig_ok = Value('b', False)
    sig_term = Value('b', False)
    parent_conn, child_conn = Pipe()

    p = Process(target=camera_proc, args=(child_conn, sig_cap, sig_ok, sig_term))
    p.start()

    #
    # Common configuration loading
    #
    config = configparser.ConfigParser()
    config.read("../configs/vision_pretreat.ini")
    pretreater = Pretreat(config)
    solver = Img2Status("../vision/ClfTrain/now.model")

    print('preloaded in ', datetime.now() - start_time)

    is_start()
    # print("sleep +1s to simulate hardware waiting")
    # time.sleep(1)
    start_time = datetime.now()
    total_time = datetime.now()
    print("start!")

    while sig_ok.value == False:
        time.sleep(0.05)
        print("camera not ok")

    # Actually, I found the original design quite sophisticated
    # I have to use another main loop to take care of the errors
    while True:
        sig_cap.value = True
        time.sleep(0.02)
        pics = parent_conn.recv()
#        cv2.imshow('up1', pics[0])
#        cv2.imshow('down1', pics[1])
        rotate_frame2()
        rotate_finish()

        time.sleep(0.5)
        
        sig_cap.value = True
        time.sleep(0.02)
        pics2 = parent_conn.recv()
#        cv2.imshow('up2', pics2[0])
#        cv2.imshow('down2', pics2[1])
#        cv2.waitKey(0)
#        cv2.destroyAllWindows()
        pics.extend(pics2)

        print("start solve......")

        # for i in range(4):
            # cv2.imwrite("./image/%d.jpg" % (i), pics[i])

        print("Taking Picture Cost: {}".format(datetime.now() - start_time))
        ################
        # Process Part #
        ################

        # image process part
        print("image process start")
        start_time = datetime.now()
        color_vectors = pretreater.GetResult(pics)
        solver.GetResult(color_vectors)
        print("process color cost:\t", (datetime.now() - start_time))

        # min2phase part
        print("min2phase start")
        start_time = datetime.now()
        cube_status = solver.status
        # with open("status.txt", "a+") as f:
        #    f.write(cube_status)
        #    f.write("\n")
        print(cube_status)

        print("U:",cube_status.count("U"),end=" ")
        print("D:",cube_status.count("D"),end=" ")
        print("R:",cube_status.count("R"),end=" ")
        print("L:",cube_status.count("L"),end=" ")
        print("F:",cube_status.count("F"),end=" ")
        print("B:",cube_status.count("B"))
#        solver.ToPics(pretreater.perspectived_imgs)

        min2phase_res = getSolution(cube_status)
        if min2phase_res.find("Error") != -1:
            # solver.ToPics(pretreater.perspectived_imgs)
            print(f"min2phase error: {min2phase_res}, rotating...")
            nothing()
            continue

        moves = getFinalResult(min2phase_res)

        print("step number:\t\t",int(moves.split()[2],16)*2)
        if int(moves.split()[2],16)*2 == 456:
            # solver.ToPics(pretreater.perspectived_imgs)
            print(f"transform error: {moves}, rotating...")
            nothing()
            continue
        
        sig_term.value = True
        
        print("min2phase cost:\t\t", (datetime.now() - start_time))
        rotate_finish()
        print("total cost:\t\t", (datetime.now() - total_time))
        break


if __name__ == '__main__':
    main()
